/**
 * @file dev_mpu6050.h
 * @brief 
 * 
 * @author dalin (dalin@robot.com)
 * @version 1.0
 * @date 2023-06-03
 * 
 * @copyright Copyright (c) 2023  OpenSir Tech.co, Ltd
 * 
 * @par 修改日志:
 * Date           Version     Author      Description
 * 2023-06-03     1.0         yangsl      touch
 */


#ifndef _DEV_MPU6050_H
#define _DEV_MPU6050_H

#include "dev_config.h"
#include "drv_i2c.h"

#define SMPLRT_DIV   0x19  // 采样率分频，典型值：0x07(125Hz) */
#define CONFIG       0x1A  // 低通滤波频率，典型值：0x06(5Hz) */
#define GYRO_CONFIG  0x1B  // 陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s) */
#define ACCEL_CONFIG 0x1C  // 加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz) */

#define ACCEL_XOUT_H 0x3B  // 存储最近的X轴、Y轴、Z轴加速度感应器的测量值 */
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40

#define TEMP_OUT_H   0x41  // 存储的最近温度传感器的测量值 */
#define TEMP_OUT_L   0x42

#define GYRO_XOUT_H  0x43  // 存储最近的X轴、Y轴、Z轴陀螺仪感应器的测量值 */
#define GYRO_XOUT_L  0x44 
#define GYRO_YOUT_H  0x45
#define GYRO_YOUT_L  0x46
#define GYRO_ZOUT_H  0x47
#define GYRO_ZOUT_L  0x48

#define PWR_MGMT_1   0x6B   // 电源管理，典型值：0x00(正常启用) */
#define WHO_AM_I     0x75 	// IIC地址寄存器(默认数值0x68，只读) */
#define MPU6050_ADDR 0xD0	// MPU6050手册上的地址，这里也可以使用serch函数去搜索


#define gyro_raw_to_deg_s       0.0609756097561f   /* +-250°/s:131LSB/°/s   +-500°/s:65.5LSB/°/s   +-1000°/s:32.8LSB/°/s    +-2000°/s:16.4LSB/°/s(本次所用) */
#define acc_raw_to_g            0.000244140625f    /* +-2g : 16384LSB/g     +-4g : 8192LSB/g   +-8g : 4096LSB/g(本次所用)   +-16g : 2048LSB/g */
#define deg_to_rad              (PI / 180.0f)
#define rad_to_angle            (180.0f / PI)                    
#define gyro_raw_to_radian_s	(gyro_raw_to_deg_s * deg_to_rad)
#define accmax_1g      4096
#define gravity_mss    9.80665f                    /* acceleration due to gravity in m/s/s */
#define acc_to_1g      gravity_mss / accmax_1g
#define one_g_to_acc   accmax_1g / gravity_mss


typedef struct 
{
	float _x;
	float _y;
	float _z;
}mpu_xyz_t;

typedef struct
{
  float yaw;
  float pitch;
  float roll;
}imuAngle_t;


typedef struct
{
	mpu_xyz_t 	accel;				// 加速度
	mpu_xyz_t	accel_offset;		// 
	mpu_xyz_t 	gyro;				// 角加速度
	mpu_xyz_t	angle;				// 角度
	float 		temp;				// 温度

	float att_acc_factor;
  	float fix_acc_factor;
}mpu6050Data_t;

typedef struct 
{
  float ax;
  float ay;
  float az;
  float gx;
  float gy;
  float gz;
  float pit;
  float rol;
  float yaw;
  float pit_mechineMedian;
  float rol_mechineMedian;
  float yaw_mechineMedian;
}_Attitude;

typedef struct
{
  float DCM[3][3];        //机体坐标系 -> 地理坐标系
  float DCM_T[3][3];      //地理坐标系 -> 机体坐标系
}_Matrix;
// int16_t sensor_i2c2_serch(void);

int8_t mpu6050_open(void);
void mpu6050_read_accel(void *data);
void mpu6050_read_gyro(void *data);
void mpu6050_read_temp(void *data);

void mahony_update(mpu_xyz_t *gyro, mpu_xyz_t *acc, imuAngle_t *angle) ;
void rotation_matrix(void);
void rotation_matrix_T(void);
void matrix_ready(void);

void get_imu_angle(void * data);
#endif


